#include "MeasurE452AlgBase.h"

once_flag MeasurE452AlgBase::m_flag;
std::vector< std::shared_ptr<std::condition_variable>> MeasurE452AlgBase::cvarptrVec;
std::vector < std::shared_ptr<std::mutex>  > MeasurE452AlgBase::mtxptrVec;
std::vector <bool> MeasurE452AlgBase::flagVec;
//std::vector< std::shared_ptr<std::condition_variable>> AlgBase::cvarptrfrontVec;
//std::vector < std::shared_ptr<std::mutex>  > AlgBase::mtxptrfrontVec;
//std::vector <bool> AlgBase::flagfrontVec;
//std::vector< std::shared_ptr<std::condition_variable>> AlgBase::cvarptrsideVec;
//std::vector < std::shared_ptr<std::mutex>  > AlgBase::mtxptrsideVec;
//std::vector <bool> AlgBase::flagsideVec;
std::vector<std::vector<Point2f>> MeasurE452AlgBase::datumBackVec;
//std::vector<std::vector<Point2f>> AlgBase::datumFrontVec;
//std::vector<std::vector<Point2f>> AlgBase::datumSideVec;
std::vector <ParameterClassE452> MeasurE452AlgBase::m_dataAllVec;

once_flag                                 MeasurE452AlgBase::m_setStandardValOnceFlag         ;
std::string                               MeasurE452AlgBase::m_dir_edge_extraction_parameters ;
std::string                               MeasurE452AlgBase::m_dir_template_classifier_model  ;
std::string                               MeasurE452AlgBase::m_dir_template_feature_model     ;
std::string                               MeasurE452AlgBase::m_dir_template_images            ;
std::map<string, float>                   MeasurE452AlgBase::m_standardValMap                 ;
string                                    MeasurE452AlgBase::m_dir_distance_fitting_model     ;
std::map<std::string, std::vector<float>> MeasurE452AlgBase::m_phypixpara[4]                  ;

MeasurE452AlgBase::MeasurE452AlgBase(int n)
{
    std::call_once(m_flag, InitFunAll, n);
//    InitFunAll(n);
}

MeasurE452AlgBase::~MeasurE452AlgBase()
{

}
void MeasurE452AlgBase::setNumberOfAlg(const int n)
{
    datumBackVec.resize(n);
    //datumFrontVec.resize(n);
    //datumSideVec.resize(n);
    MeasurE452AlgBase::cvarptrVec.resize(n);
    MeasurE452AlgBase::mtxptrVec.resize(n);
    MeasurE452AlgBase::flagVec.resize(n);
//    AlgBase::cvarptrfrontVec.resize(n);
//    AlgBase::mtxptrfrontVec.resize(n);
//    AlgBase::flagfrontVec.resize(n);
//    AlgBase::cvarptrsideVec.resize(n);
//    AlgBase::mtxptrsideVec.resize(n);
//    AlgBase::flagsideVec.resize(n);
    for (size_t i = 0; i < MeasurE452AlgBase::flagVec.size(); i++)
    {
        MeasurE452AlgBase::flagVec[i] = false;
        std::shared_ptr<std::condition_variable> cvar(new std::condition_variable());
        std::shared_ptr<std::mutex> mtx(new std::mutex());
        cvarptrVec[i]=cvar;
        mtxptrVec[i] = mtx;

//        AlgBase::flagfrontVec[i] = false;
//        std::shared_ptr<std::condition_variable> cvarfront(new std::condition_variable());
//        std::shared_ptr<std::mutex> mtxfront(new std::mutex());
//        cvarptrfrontVec[i] = cvarfront;
//        mtxptrfrontVec[i] = mtxfront;

//        AlgBase::flagsideVec[i] = false;
//        std::shared_ptr<std::condition_variable> cvarside(new std::condition_variable());
//        std::shared_ptr<std::mutex> mtxside(new std::mutex());
//        cvarptrsideVec[i] = cvarside;
//        mtxptrsideVec[i] = mtxside;

    }

}

void MeasurE452AlgBase::InitFunAll(const int n)
{
    setNumberOfAlg(n);

    {
        //back circle L12345678-> 01234567
        std::vector<int> toll{0,1,2,3,4,5,6,7};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem+1));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname=m_dir_template_images+backImg+std::string("L")+std::to_string((elem+1))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("L.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });

    }

    {

        //back circle R1234-> 891011
        std::vector<int> toll{8,9,10,11};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem+1));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname=m_dir_template_images+backImg+std::string("R")+std::to_string((elem-7))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("R.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });

    }

    {
        //back circle D12345678-> 12, 13, 14, 15, 16,17,18,19
        std::vector<int> toll{12, 13, 14, 15, 16,17,18,19};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem+1));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname=m_dir_template_images+backImg+std::string("D")+std::to_string((elem-11))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("D.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });

    }

    {
        //back SS1SS2->  20 21
        std::vector<int> toll{ 20, 21};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem+1));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname=m_dir_template_images+backImg+std::string("SS")+std::to_string((elem-19))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("SS.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });
    }

    {
        //back  S1S2S4S5-> 22,23,24,25
        std::vector<int> toll{22,23,24,25};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem+1));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname;
            if(elem<24)
            {imgname=m_dir_template_images+backImg+std::string("S")+std::to_string((elem-21))+backImgTmp;}
            else
            {imgname=m_dir_template_images+backImg+std::string("S")+std::to_string((elem-20))+backImgTmp;}
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("S.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });
    }

    {
        //front SE123456789-> 26,27,28,29,30,31,32,33,34
        std::vector<int> toll{26,27,28,29,30,31,32,33,34};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("front.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem-25));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("front/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname=m_dir_template_images+backImg+std::string("SE")+std::to_string((elem-25))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("SE.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });
    }

    {
        //high  SS3,SS4,SS5,SS6,SS7,S3-> 35,36,37,38,39,40
        std::vector<int> toll{35,36,37,38,39,40};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("high.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem-34));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("high/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname;
            if(elem<40)
            {imgname=m_dir_template_images+backImg+std::string("SS")+std::to_string((elem-32))+backImgTmp;}
            else
            {imgname=m_dir_template_images+backImg+std::string("S")+std::to_string((elem-37))+backImgTmp;}
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("SS.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });
    }
    {
        //back setup123 ->41,42,43
        std::vector<int> toll{41,42,43};
        for_each(toll.begin(),toll.end(),[](int &elem){
            int regionNum=elem;
            std::string  backxml("back.xml");
            cv::FileStorage fs((m_dir_template_images+backxml).c_str(),cv::FileStorage::READ);
            std::string tagname=std::string("temp_info")+std::to_string((elem-14));
            cv::Rect regionRect;
            fs[tagname][std::string("match_region")]>>regionRect;
            fs.release();
            std::string  backImg("back/"),backImgTmp("/best_temp.png");
            std::vector<cv::Mat> imgVec;
            std::string imgname = m_dir_template_images+backImg+std::string("Setup")+std::to_string((elem-40))+backImgTmp;
            cv::Mat img=cv::imread(imgname.c_str(),0);
            imgVec.push_back(img.clone());
            cv::FileStorage fse((m_dir_edge_extraction_parameters+backImg+std::string("Setup.xml")).c_str(),cv::FileStorage::READ);
            ParameterClassE452 obj;
            obj.m_regionNum=regionNum;
            obj.m_templateImageVec=imgVec;
            obj.m_regionRect=regionRect;
            fse["voc"]>>obj.m_edgeParam;
            fse.release();
            m_dataAllVec.push_back(obj);
        });
    }

    // load fit param
    if(!LoadFitPara()) {
        qCritical() << "量测算法：E452加载参数失败";
        return;
    }


}

bool MeasurE452AlgBase::InitPath(const string &path)
{
    m_dir_edge_extraction_parameters = (path + "/E452/452MeasureData/edge_extraction_parameters/");
    m_dir_template_classifier_model  = (path + "/E452/452MeasureData/template_classifier_model/") ;
    m_dir_template_feature_model     = (path + "/E452/452MeasureData/template_feature_model/");
    m_dir_template_images            = (path + "/E452/452MeasureData/template_images/");
    m_dir_distance_fitting_model     = (path + "/E452/452MeasureData/distance_fitting_model/");

    qDebug() << "init path: " << QString::fromStdString(m_dir_distance_fitting_model);

    auto isDirExist = [](const std::string &dirName) {
        QDir dir(QString::fromStdString(dirName));
        if (!dir.exists()) {
            qCritical() << QString("灰度算法：量测算法E452配置文件路径不存在：%1").arg(QString::fromStdString(dirName));
            return false;
        } else {
            return true;
        }
    };

    if (!isDirExist(m_dir_edge_extraction_parameters)
            || !isDirExist(m_dir_template_classifier_model)
            || !isDirExist(m_dir_template_feature_model)
            || !isDirExist(m_dir_template_images)
            || !isDirExist(m_dir_distance_fitting_model))
    {
        return false;
    }
    qDebug() << "init path success " ;
    return true;
}

bool MeasurE452AlgBase::SetTemplatePath(const string &path)
{
    static bool called      = false;
    static bool initResult  = false;
    if(!called) {
        initResult = InitPath(path);
        called = true;
    }
    return initResult;
}

void MeasurE452AlgBase::SetStandardVal(const std::map<string, float> standardVal)
{
    std::call_once(m_setStandardValOnceFlag, InitStandardVal, standardVal);
}

bool MeasurE452AlgBase::LoadFitPara()
{
    qDebug() << "************load fit para start**************";
    {
        // read fit parameter
        vector<std::string> pyhpixstr{"fitpara0.xml", "fitpara1.xml", "fitpara2.xml", "fitpara3.xml"};
        std::vector<std::string> nameList = {
            "FAI24-D-diam",
            "FAI25-D-X-distance",
            "FAI25-D-Y-distance",
            "FAI25-D-position",
            "FAI21-L-diam",
            "FAI22-L-X-distance",
            "FAI22-L-Y-distance",
            "FAI22-L-position",
            "FAI18-R-diam",
            "FAI19-R-X-distance",
            "FAI19-R-Y-distance",
            "FAI19-R-position",
            "FAI29-S1-X-distance",
            "FAI30-S1-Y-distance",
            "FAI31-S4P9-X-distance",
            "FAI31-S5P10-X-distance",
            "FAI160-S4P9-X-distance",
            "FAI160-S5P10-X-distance",
            "FAI52-S2-X-distance",
            "FAI53-S2-Y-distance",
            "FAI58-S2-Y-distance",
            "FAI59-S2S3-X-distance",
            "FAI60-S2-X-distance",
            "FAI113-SS1-X-distance",
            "FAI120-SS1-Y-distance",
            "SS1-diam",
            "FAI114-SS2-X-distance",
            "FAI121-SS2-Y-distance",
            "SS2-diam",
            "FAI4-SE2SE6-distance",
            "FAI5-SE3SE9-distance",
            "FAI6-SE7-distance",
            "FAI7-SE5-distance",
            "FAI8-SE1SE8-distance",
            "FAI9-SE4-distance",
            "FAI115-SS3-X-distance",
            "FAI116-SS4-X-distance",
            "FAI117-SS5-X-distance",
            "FAI118-SS6-X-distance",
            "FAI119-SS7-X-distance",
            "FAI122-SS3-Y-distance",
            "FAI123-SS4-Y-distance",
            "FAI124-SS5-Y-distance",
            "FAI124-SS6-Y-distance",
            "FAI125-SS7-Y-distance",
            "SS3-diam",
            "SS4-diam",
            "SS5-diam",
            "SS6-diam",
            "SS7-diam",
            "FAI154-SS1SS4-X-distance",
            "FAI154-SS1SS4-Y-distance",
            "FAI155-SS6SS4-X-distance",
            "FAI155-SS6SS4-Y-distance",
            "FAI154-SS1SS4-position",
            "FAI155-SS6SS4-position"
        };

        auto LoadFitPara_ = [&](const FileStorage& fs,
                const std::string &name,
                std::map<std::string, std::vector<float>>& phypixpara)
        {
            for(size_t i = 0; i < 3; i++)
            {
                phypixpara[std::string(name)].push_back(static_cast<float>(fs[std::string(name)][i]));
            }
        };

        for(size_t i = 0; i < pyhpixstr.size(); i++) {
            FileStorage fs((m_dir_distance_fitting_model + pyhpixstr[i]).c_str(),FileStorage::READ);
            qDebug() << "fit para path: " << QString::fromStdString(m_dir_distance_fitting_model + pyhpixstr[i]);

            for(auto &item : nameList) {
                LoadFitPara_(fs, item, m_phypixpara[i]);
            }
            fs.release();
        }
    }

    return true;
    qDebug() << "load fit para end";
}

void MeasurE452AlgBase::InitStandardVal(const std::map<string, float> standardVal)
{
    for (auto &iter : standardVal) {
        m_standardValMap[iter.first] = iter.second;
    }
}

float MeasurE452AlgBase::CalcMeasureDistance(const vector<float> &coefficient, float measureItem)
{
    if(coefficient.empty()) {
        qCritical() << "empty coefficient";
        Q_ASSERT(coefficient.size() != 0);
        return -1;
    }

    return coefficient[0]*measureItem*measureItem + coefficient[1]*measureItem + coefficient[2];
}

void MeasurE452AlgBase::TemplateMatch(cv::Mat &img, ParameterClassE452 &obj,
                                      std::vector<cv::Rect> &rangVec)
{

        cv::Mat templ=obj.m_templateImageVec[0];
        int result_cols =  img.cols - templ.cols + 1;
        int result_rows = img.rows - templ.rows + 1;
        cv::Mat result;
        result.create( result_cols, result_rows, CV_32FC1 );
        cv::Mat imgRoi=img(obj.m_regionRect);
        matchTemplate( imgRoi, templ, result,  CV_TM_CCOEFF_NORMED );
        normalize( result, result, 0, 1, NORM_MINMAX, -1, cv::Mat() );
        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc;
        cv::Point matchLoc;
        minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
        matchLoc.x=maxLoc.x+obj.m_regionRect.x;
        matchLoc.y=maxLoc.y+obj.m_regionRect.y;
        cv::Rect rz;
        rz.x=matchLoc.x,rz.y=matchLoc.y,rz.width=templ.cols,rz.height=templ.rows;
        rangVec.push_back(rz);
}

void MeasurE452AlgBase::CoordinateTransformation(std::vector<cv::Point2f> &ptsVec, cv::Point2f &ptsIn,cv::Point2f &ptsOut)
{
    float A1=0.0,B1=0.0,C1=0.0;
    float A2=0.0,B2=0.0,C2=0.0;
    A1=ptsVec[1].y-ptsVec[0].y;
    B1=ptsVec[0].x-ptsVec[1].x;
    C1=ptsVec[0].y*ptsVec[1].x-ptsVec[0].x*ptsVec[1].y;
    ptsOut.y=abs(A1*ptsIn.x+B1*ptsIn.y+C1)/sqrt(A1*A1+B1*B1);

    A2=ptsVec[2].y-ptsVec[0].y;
    B2=ptsVec[0].x-ptsVec[2].x;
    C2=ptsVec[0].y*ptsVec[2].x-ptsVec[0].x*ptsVec[2].y;
    ptsOut.x=abs(A2*ptsIn.x+B2*ptsIn.y+C2)/sqrt(A2*A2+B2*B2);

}

void MeasurE452AlgBase::clearAllflag()
{
    for (size_t i = 0; i < MeasurE452AlgBase::flagVec.size(); i++)
    {
        MeasurE452AlgBase::flagVec[i] = false;
//        AlgBase::flagfrontVec[i] = false;
//        AlgBase::flagsideVec[i] = false;
    }
}

//void AlgBb vase::TemplateMatch(cv::Mat &img, int &&num, std::vector<Rect> &rangVec)
//{

// rangVec.resize(num.m_templateImageVec.size());
//#pragma omp parallel for num_threads(6)
//    for(int i=0;i<int(num.m_templateImageVec.size());i++)
//    {
//        cv::Mat templ=num.m_templateImageVec[i];
//        int result_cols =  img.cols - templ.cols + 1;
//        int result_rows = img.rows - templ.rows + 1;
//        cv::Mat result;
//        result.create( result_cols, result_rows, CV_32FC1 );
//        cv::Mat imgRoi=img(num.m_regionRect);
//        matchTemplate( imgRoi, templ, result,  CV_TM_CCOEFF_NORMED );
//        normalize( result, result, 0, 1, NORM_MINMAX, -1, cv::Mat() );
//        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc;
//        cv::Point matchLoc;
//        minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
//        matchLoc=maxLoc;
//        cv::Rect rz;
//        rz.x=matchLoc.x,rz.y=matchLoc.y,rz.width=templ.cols,rz.height=templ.rows;
//        rangVec.push_back(rz);
//    }

//}

